#include "chassis_task.h"
#include "CAN_receive.h"
#include "NUC_communicate.h"
#include "OLED.h"
#include "arm_math.h"
#include "cmsis_os.h"
#include "detect_task.h"
#include "pid.h"
#include "referee.h"
#include "remote_control.h"
#include "robot_config.h"
#include "robot_total_mode_task.h"
#include "INS_task.h"
#include "shoot_task.h" 
#include "user_lib.h"

// 预先测量的云台在中心时的yawECD。在机器人重新安装云台后，可能导致ECD和这个不一样
#ifdef INFANTRY
#define YAW_MID_ECD 1744
#endif
#ifdef HERO
#define YAW_MID_ECD 72
#endif
#ifdef SENTRY
#define CHASSIS_LF_G_MID_ECD 2657
#define CHASSIS_RF_G_MID_ECD 7461
#define CHASSIS_RB_G_MID_ECD 3992
#define CHASSIS_LB_G_MID_ECD 7602
#endif

#define CHASSIS_TASK_INIT_TIME 500  // 电机初始化时间
#define CHASSIS_CONTROL_TIME_MS 5   // 每5ms执行一次任务

#define rc_deadband_limit(input, output, dealine)          \
    {                                                      \
        if ((input) > (dealine) || (input) < -(dealine)) { \
            (output) = (input);                            \
        } else {                                           \
            (output) = 0;                                  \
        }                                                  \
    }
#define range_limit_inside(toBeLimited, range)                      \
    {                                                               \
         if((toBeLimited)>(range))                                  \
        {                                                           \
            (toBeLimited)=(range);                                  \
        }                                                           \
        else if((toBeLimited)<-(range))                             \
        {                                                           \
            (toBeLimited)=-(range);                                 \
        }                                                           \
    }
/**********************PID控制参数********************/
// 底盘电机速度环PID
#ifdef INFANTRY
#define M3508_MOTOR_SPEED_PID_KP 60.0f
#define M3508_MOTOR_SPEED_PID_KI 0.0f
#define M3508_MOTOR_SPEED_PID_KD 0.0f
#define M3508_MOTOR_SPEED_PID_MAX_OUT 15000.0f
#define M3508_MOTOR_SPEED_PID_MAX_IOUT 2000.0f
#endif

#ifdef HERO
#define M3508_MOTOR_SPEED_PID_KP 50.0f
#define M3508_MOTOR_SPEED_PID_KI 0.0f
#define M3508_MOTOR_SPEED_PID_KD 0.0f
#define M3508_MOTOR_SPEED_PID_MAX_OUT 15000.0f
#define M3508_MOTOR_SPEED_PID_MAX_IOUT 2000.0f
#endif

#ifdef SENTRY_CHASSIS
#define M3508_MOTOR_SPEED_PID_KP 25.0f
#define M3508_MOTOR_SPEED_PID_KI 1.0f
#define M3508_MOTOR_SPEED_PID_KD 0.0f
#define M3508_MOTOR_SPEED_PID_MAX_OUT 15000.0f
#define M3508_MOTOR_SPEED_PID_MAX_IOUT 4000.0f
#define M3508_MOTOR_SPEED_FILTER 2

#define GM6020_1_CHESSIS_AGL_KP 100.0f
#define GM6020_1_CHESSIS_AGL_KI 0.0f
#define GM6020_1_CHESSIS_AGL_KD 0.0f
#define GM6020_CHESSIS_AGL_MAXOUT 50.0f
#define GM6020_CHESSIS_AGL_MAX_IOUT 0.7f

#define GM6020_2_CHESSIS_AGL_KP 100.0f
#define GM6020_2_CHESSIS_AGL_KI 0.0f
#define GM6020_2_CHESSIS_AGL_KD 0.0f

#define GM6020_3_CHESSIS_AGL_KP 120.0f
#define GM6020_3_CHESSIS_AGL_KI 0.0f
#define GM6020_3_CHESSIS_AGL_KD 0.0f

#define GM6020_4_CHESSIS_AGL_KP 100.0f
#define GM6020_4_CHESSIS_AGL_KI 0.0f
#define GM6020_4_CHESSIS_AGL_KD 0.0f

#define GM6020_1_CHESSIS_SPD_KP 240.0f
#define GM6020_1_CHESSIS_SPD_KI 0.0f
#define GM6020_1_CHESSIS_SPD_KD 0.0f
#define GM6020_CHESSIS_SPD_MAXOUT 15000.0f
#define GM6020_CHESSIS_SPD_MAX_IOUT 2000.0f

#define GM6020_2_CHESSIS_SPD_KP 110.0f
#define GM6020_2_CHESSIS_SPD_KI 0.0f
#define GM6020_2_CHESSIS_SPD_KD 0.0f

#define GM6020_3_CHESSIS_SPD_KP 130.0f
#define GM6020_3_CHESSIS_SPD_KI 0.0f
#define GM6020_3_CHESSIS_SPD_KD 0.0f

#define GM6020_4_CHESSIS_SPD_KP 90.0f
#define GM6020_4_CHESSIS_SPD_KI 0.0f
#define GM6020_4_CHESSIS_SPD_KD 0.0f

#endif

/******************数字滤波所需常量****************/
// 系数和控制方法来自官方步兵
#define CHASSIS_ACCEL_SPEED_NUM 150.1666f
#define CHASSIS_ACCEL_W_NUM 150.66f

//******************功率控制相关常量***********************//   // 2.3
#define FULL_BUFFER         60.0f   // 最大缓冲能量
#define DANGEROUS_BUFFER    20.0f    // 危险缓冲能量，绝对不能让缓冲能量低于这个值

#define CURRENT_TO_DATA     819.2f  // 每A电流对应发送的数据。物理的安培对应的CAN数据
#define VOLTAGE             24.0f   // 供电电压为24V

#define COMMUNICATING_PERIOD_S  0.1f     // 与裁判系统的通讯周期。单位为s

#define abss(input, output)     \
{                               \
    if ((input) >= 0)             \
        (output) = (input);         \
    else                        \
        (output) = -(input);        \
}

/**************************控制量结构体*******************/
// 机器人控制量
struct robotControl_t {
    fp32 vx, vy, w;
    first_order_filter_type_t vx_filter, vy_filter, w_filter;
    fp32 gimbalAngleFromChassis;  // 云台相对于最初的角度，逆时针为正方向。单位弧度。
    #ifndef SENTRY
    int16_t nowYawECD;            // 目前的ECD
    #endif
};
#ifdef STEERING_WHEELS
// 舵轮控制
struct AGVControl_t {
    fp32 vx;                    // x方向速度
    fp32 vy;                    // y方向速度
    fp32 presentAngle;          // 当前角度
    fp32 wantedAngle;           // 期望角度
    fp32 presentMotorSpeed;     // 当前速度
    fp32 presentMotorSpeed_GM;  // GM6020当前速度
    fp32 wantedMotorSpeed;      // 期望速度
    int8_t directionFlag;       // directionFlag*vx=实际vx
    first_order_filter_type_t M3508SpeedFilter;
};

// 底盘M3508控制
struct MotorControl_M_t {
    pid_type_def vpid;
    int16_t giveCurrent;
};

// 底盘GM6020控制
struct MotorControl_GM_t {
    pid_type_def vpidOfRpm;
    pid_type_def vpidOfVolage;
    int16_t giveCurrent;
};
#else
// 底盘电机控制
struct MotorControl_t {
    pid_type_def vpid;
    fp32 presentMotorSpeed;
    fp32 wantedMotorSpeed;
    int16_t giveCurrent;
};
#endif
/*************************用于控制的全局变量****************/
static int8_t zeroCurrentMark;  // 当离线时，直接发送零电流的时候
#ifdef STEERING_WHEELS
static struct MotorControl_M_t driveMotor_M[4];    // M3508
static struct MotorControl_GM_t driveMotor_GM[4];  // GM6020
static struct AGVControl_t AGVControl[4];          // 舵轮控制
#else
static struct MotorControl_t driveMotor[4];
#endif
static const RC_ctrl_t* rc_p;     // 遥控器位置指针，需要初始化
static const fromNUC_t* nuc_p;    // NUC数据位置
static const remote_control_t* remoteControl_p;//图传链路控制
static const uint8_t* robotMode;  // 机器人模式
static struct robotControl_t robotTotalSpeedControl;
static int16_t vx_channel, vy_channel, w_channel;  // 遥控器deadband limit后输出的值
#ifdef STEERING_WHEELS
static fp32 motor_M_speed_pid[3] = {M3508_MOTOR_SPEED_PID_KP, M3508_MOTOR_SPEED_PID_KI, M3508_MOTOR_SPEED_PID_KD};  // PID 初始化参数
static fp32 motor_GM_1_ang_pid[3] = {GM6020_1_CHESSIS_AGL_KP, GM6020_1_CHESSIS_AGL_KI, GM6020_1_CHESSIS_AGL_KD};    // GM6020速度环PID 初始化参数
static fp32 motor_GM_2_ang_pid[3] = {GM6020_2_CHESSIS_AGL_KP, GM6020_2_CHESSIS_AGL_KI, GM6020_2_CHESSIS_AGL_KD};    // GM6020速度环PID 初始化参数
static fp32 motor_GM_3_ang_pid[3] = {GM6020_3_CHESSIS_AGL_KP, GM6020_3_CHESSIS_AGL_KI, GM6020_3_CHESSIS_AGL_KD};    // GM6020速度环PID 初始化参数
static fp32 motor_GM_4_ang_pid[3] = {GM6020_4_CHESSIS_AGL_KP, GM6020_4_CHESSIS_AGL_KI, GM6020_4_CHESSIS_AGL_KD};    // GM6020速度环PID 初始化参数
static fp32 motor_GM_1_speed_pid[3] = {GM6020_1_CHESSIS_SPD_KP, GM6020_1_CHESSIS_SPD_KI, GM6020_1_CHESSIS_SPD_KD};  // GM6020电压环PID 初始化参数
static fp32 motor_GM_2_speed_pid[3] = {GM6020_2_CHESSIS_SPD_KP, GM6020_2_CHESSIS_SPD_KI, GM6020_2_CHESSIS_SPD_KD};  // GM6020电压环PID 初始化参数
static fp32 motor_GM_3_speed_pid[3] = {GM6020_3_CHESSIS_SPD_KP, GM6020_3_CHESSIS_SPD_KI, GM6020_3_CHESSIS_SPD_KD};  // GM6020电压环PID 初始化参数
static fp32 motor_GM_4_speed_pid[3] = {GM6020_4_CHESSIS_SPD_KP, GM6020_4_CHESSIS_SPD_KI, GM6020_4_CHESSIS_SPD_KD};  // GM6020电压环PID 初始化参数
#else
#ifndef SENTRY_GIMBAL
static fp32 motor_speed_pid[3] = {M3508_MOTOR_SPEED_PID_KP, M3508_MOTOR_SPEED_PID_KI, M3508_MOTOR_SPEED_PID_KD};  // PID 初始化参数
#endif
#endif
#ifdef SENTRY
static betweenTwoSTM_t* betweenTwoSTMMessage_p;
static const fp32* yawAngle_p;
static fp32 initYawMid;
#endif
#ifdef WITH_REFEREE 
fp32 powerNow;     // 上一次裁判系统读到的底盘总功率
fp32 bufferNow = 60.0f;     // 当前剩余的缓冲能量
fp32 totalCurrentLimit=0.0f;     // 底盘电流上限
#endif

static void PIDs_init(void) {
    int i;
    for (i = 0; i < 4; i++) {
#ifdef STEERING_WHEELS
        PID_init(&(driveMotor_M[i].vpid), PID_POSITION, motor_M_speed_pid, M3508_MOTOR_SPEED_PID_MAX_OUT, M3508_MOTOR_SPEED_PID_MAX_IOUT);
        if (i == 0) {
            PID_init(&(driveMotor_GM[i].vpidOfRpm), PID_POSITION, motor_GM_1_ang_pid, GM6020_CHESSIS_AGL_MAXOUT, GM6020_CHESSIS_AGL_MAX_IOUT);
            PID_init(&(driveMotor_GM[i].vpidOfVolage), PID_POSITION, motor_GM_1_speed_pid, GM6020_CHESSIS_SPD_MAXOUT, GM6020_CHESSIS_SPD_MAX_IOUT);
        }
        if (i == 1) {
            PID_init(&(driveMotor_GM[i].vpidOfRpm), PID_POSITION, motor_GM_2_ang_pid, GM6020_CHESSIS_AGL_MAXOUT, GM6020_CHESSIS_AGL_MAX_IOUT);
            PID_init(&(driveMotor_GM[i].vpidOfVolage), PID_POSITION, motor_GM_2_speed_pid, GM6020_CHESSIS_SPD_MAXOUT, GM6020_CHESSIS_SPD_MAX_IOUT);
        }
        if (i == 2) {
            PID_init(&(driveMotor_GM[i].vpidOfRpm), PID_POSITION, motor_GM_3_ang_pid, GM6020_CHESSIS_AGL_MAXOUT, GM6020_CHESSIS_AGL_MAX_IOUT);
            PID_init(&(driveMotor_GM[i].vpidOfVolage), PID_POSITION, motor_GM_3_speed_pid, GM6020_CHESSIS_SPD_MAXOUT, GM6020_CHESSIS_SPD_MAX_IOUT);
        }
        if (i == 3) {
            PID_init(&(driveMotor_GM[i].vpidOfRpm), PID_POSITION, motor_GM_4_ang_pid, GM6020_CHESSIS_AGL_MAXOUT, GM6020_CHESSIS_AGL_MAX_IOUT);
            PID_init(&(driveMotor_GM[i].vpidOfVolage), PID_POSITION, motor_GM_4_speed_pid, GM6020_CHESSIS_SPD_MAXOUT, GM6020_CHESSIS_SPD_MAX_IOUT);
        }
#else
#ifndef SENTRY_GIMBAL
        PID_init(&(driveMotor[i].vpid), PID_POSITION, motor_speed_pid, M3508_MOTOR_SPEED_PID_MAX_OUT, M3508_MOTOR_SPEED_PID_MAX_IOUT);
#endif
#endif
    }
}

static void Filters_init(void) {
    const static fp32 chassis_v_order_filter[1] = {CHASSIS_ACCEL_SPEED_NUM};
    const static fp32 chassis_w_order_filter[1] = {CHASSIS_ACCEL_W_NUM};
    #ifdef SENTRY_CHASSIS
    const static fp32 chassisM3508SpeedFilter[1]={M3508_MOTOR_SPEED_FILTER};
    #endif

    first_order_filter_init(&(robotTotalSpeedControl.vx_filter), CHASSIS_CONTROL_TIME_MS, chassis_v_order_filter);
    first_order_filter_init(&(robotTotalSpeedControl.vy_filter), CHASSIS_CONTROL_TIME_MS, chassis_v_order_filter);
    first_order_filter_init(&(robotTotalSpeedControl.w_filter), CHASSIS_CONTROL_TIME_MS, chassis_w_order_filter);
    #ifdef SENTRY_CHASSIS
    for(uint8_t i=0;i<4;i++){
        first_order_filter_init(&(AGVControl[i].M3508SpeedFilter), CHASSIS_CONTROL_TIME_MS, chassisM3508SpeedFilter);
    }
    #endif


}

static void first_order_filt() {
   first_order_filter_cali(&robotTotalSpeedControl.vx_filter, robotTotalSpeedControl.vx);
   first_order_filter_cali(&robotTotalSpeedControl.vy_filter, robotTotalSpeedControl.vy);
   first_order_filter_cali(&robotTotalSpeedControl.w_filter, robotTotalSpeedControl.w);
   robotTotalSpeedControl.vx = robotTotalSpeedControl.vx_filter.out;
   robotTotalSpeedControl.vy = robotTotalSpeedControl.vy_filter.out;
   robotTotalSpeedControl.w = robotTotalSpeedControl.w_filter.out;
}


static void analyse_total_control(void)  // 通过遥控器和键盘参数设置速度。当键盘输出量为0时，通过遥控器摇杆确定速度。
{
    zeroCurrentMark = 0;
    robotTotalSpeedControl.vx = 0;
    robotTotalSpeedControl.vy = 0;
    robotTotalSpeedControl.w = 0;
    if (*robotMode == ROBOT_STATE_POWERLESS)
        zeroCurrentMark = 1;
    else if (*robotMode == ROBOT_STATE_AUTO) {
#ifdef WITH_REFEREE
        if (game_not_started())
            zeroCurrentMark = 1;
#endif
    }
#ifdef SENTRY_CHASSIS

    if (ROBOT_STATE_POWERLESS == *robotMode) {
        robotTotalSpeedControl.vx = 0;
        robotTotalSpeedControl.vy = 0;
        robotTotalSpeedControl.w = 0;
        return;
    } else if (ROBOT_STATE_AUTO == *robotMode) {
        if (!toe_is_error(MINI_PC_TOE)) {
            #ifdef USE_USART
            robotTotalSpeedControl.vx = ((fp32)nuc_p->x) * CHASSIS_MAX_SPEED / (fp32)INT16_MAX;
            robotTotalSpeedControl.vy = ((fp32)nuc_p->y) * CHASSIS_MAX_SPEED / (fp32)INT16_MAX;
            robotTotalSpeedControl.w = ((fp32)nuc_p->w) * CHASSIS_MAX_OMEGA / (fp32)INT16_MAX;
            #endif
            #ifdef USE_CAN
            robotTotalSpeedControl.vx = ((fp32)nuc_p->x) * CHASSIS_MAX_SPEED / (fp32)INT8_MAX;
            robotTotalSpeedControl.vy = ((fp32)nuc_p->y) * CHASSIS_MAX_SPEED / (fp32)INT8_MAX;
            robotTotalSpeedControl.w = ((fp32)nuc_p->w) * CHASSIS_MAX_OMEGA / (fp32)INT8_MAX;
            #endif
        } else {
            robotTotalSpeedControl.vx = 0;
            robotTotalSpeedControl.vy = 0;
            robotTotalSpeedControl.w = 0;
        }
    } else {
        if(!toe_is_error(GIMBALSTM32_TOE)){
            robotTotalSpeedControl.vx = ((fp32)betweenTwoSTMMessage_p->x) * CHASSIS_MAX_SPEED / (fp32)INT8_MAX;
            robotTotalSpeedControl.vy = ((fp32)betweenTwoSTMMessage_p->y) * CHASSIS_MAX_SPEED / (fp32)INT8_MAX;
            robotTotalSpeedControl.w = ((fp32)betweenTwoSTMMessage_p->w) * CHASSIS_MAX_OMEGA / (fp32)INT8_MAX;
        }else {
            robotTotalSpeedControl.vx = 0;
            robotTotalSpeedControl.vy = 0;
            robotTotalSpeedControl.w = 0;
        }
    }
#else
    if (*robotMode == ROBOT_STATE_POWERLESS) {
        robotTotalSpeedControl.vx = 0;
        robotTotalSpeedControl.vy = 0;
        robotTotalSpeedControl.w = 0;
        return;
    } else if (*robotMode == ROBOT_STATE_AUTO) {
        if (!toe_is_error(MINI_PC_TOE)) {
            #ifdef USE_USART
            robotTotalSpeedControl.vx = ((fp32)nuc_p->x) * CHASSIS_MAX_SPEED / (fp32)INT16_MAX;
            robotTotalSpeedControl.vy = ((fp32)nuc_p->y) * CHASSIS_MAX_SPEED / (fp32)INT16_MAX;
            robotTotalSpeedControl.w = ((fp32)nuc_p->w) * CHASSIS_MAX_OMEGA / (fp32)INT16_MAX;
            #endif
            #ifdef USE_CAN
            robotTotalSpeedControl.vx = ((fp32)nuc_p->x) * CHASSIS_MAX_SPEED / (fp32)INT8_MAX;
            robotTotalSpeedControl.vy = ((fp32)nuc_p->y) * CHASSIS_MAX_SPEED / (fp32)INT8_MAX;
            robotTotalSpeedControl.w = ((fp32)nuc_p->w) * CHASSIS_MAX_OMEGA / (fp32)INT8_MAX;
            #endif
        } else {
            robotTotalSpeedControl.vx = 0;
            robotTotalSpeedControl.vy = 0;
            robotTotalSpeedControl.w = 0;
        }
    } else if (*robotMode == ROBOT_STATE_COMMON) {
        if (key_has_changed()) {
            if (remoteControl_p->keyboard_value & CHASSIS_FORTH)  // 前
                robotTotalSpeedControl.vx = CHASSIS_KEYBOARD_SPEED;
            if (remoteControl_p->keyboard_value  & CHASSIS_BACK)  // 后
                robotTotalSpeedControl.vx = -CHASSIS_KEYBOARD_SPEED;
            if (remoteControl_p->keyboard_value  & CHASSIS_LEFT_MOVE)  // 左
                robotTotalSpeedControl.vy = CHASSIS_KEYBOARD_SPEED;
            if (remoteControl_p->keyboard_value  & CHASSIS_RIGHT_MOVE)  // 右
                robotTotalSpeedControl.vy = -CHASSIS_KEYBOARD_SPEED;
            if (remoteControl_p->keyboard_value  & CHASSIS_RIGHT_ROTATE)  // 右旋
                robotTotalSpeedControl.w =-CHASSIS_KEYBOARD_OMEGA;
            if (remoteControl_p->keyboard_value  & CHASSIS_LEFT_ROTATE)  // 左旋
                robotTotalSpeedControl.w = CHASSIS_KEYBOARD_OMEGA;
        } else {
            rc_deadband_limit(rc_p->rc.ch[RC_CHASSIS_X], vx_channel, CHASSIS_RC_DEADLINE);
            #ifndef SENTRY
            if (switch_is_up(rc_p->rc.ch[RC_CHOOSE_Y_OR_W])) {
                rc_deadband_limit(rc_p->rc.ch[RC_CHASSIS_Y_OR_W], vy_channel, CHASSIS_RC_DEADLINE);
                w_channel = 0;
            } else {
                rc_deadband_limit(rc_p->rc.ch[RC_CHASSIS_Y_OR_W], w_channel, CHASSIS_RC_DEADLINE);
                vy_channel = 0;
            }
            #else
            rc_deadband_limit(rc_p->rc.ch[RC_CHASSIS_Y_OR_W], w_channel, CHASSIS_RC_DEADLINE);
            vy_channel = 0;
            #endif
            robotTotalSpeedControl.vx = vx_channel * CHASSIS_SPEED_RC;
            robotTotalSpeedControl.vy = -vy_channel * CHASSIS_SPEED_RC;
            robotTotalSpeedControl.w =- w_channel * CHASSIS_OMEGA_RC;
        }
    } else {
        if (key_has_changed()) {
            if (remoteControl_p->keyboard_value  & CHASSIS_FORTH)  // 前
                robotTotalSpeedControl.vx = CHASSIS_MAX_SPEED;
            if (remoteControl_p->keyboard_value  & CHASSIS_BACK)  // 后
                robotTotalSpeedControl.vx = -CHASSIS_MAX_SPEED;
            if (remoteControl_p->keyboard_value  & CHASSIS_LEFT_MOVE)  // 左
                robotTotalSpeedControl.vy = CHASSIS_MAX_SPEED;
            if (remoteControl_p->keyboard_value  & CHASSIS_RIGHT_MOVE)  // 右
                robotTotalSpeedControl.vy = -CHASSIS_MAX_SPEED;
            robotTotalSpeedControl.w = CHASSIS_KEYBOARD_OMEGA;
        } else {
            rc_deadband_limit(rc_p->rc.ch[RC_CHASSIS_X], vx_channel, CHASSIS_RC_DEADLINE);
            rc_deadband_limit(rc_p->rc.ch[RC_CHASSIS_Y_OR_W], vy_channel, CHASSIS_RC_DEADLINE);
            robotTotalSpeedControl.vx = vx_channel * CHASSIS_SPEED_RC;
            robotTotalSpeedControl.vy = - vy_channel * CHASSIS_SPEED_RC;
            robotTotalSpeedControl.w = CHASSIS_MAX_OMEGA;
        }
    }
    if (remoteControl_p->keyboard_value  & USE_SUPPER_CAP) {
        robotTotalSpeedControl.vx *= CHASSIS_WITH_SUPPER_CAP;
        robotTotalSpeedControl.vy *= CHASSIS_WITH_SUPPER_CAP;
        robotTotalSpeedControl.w *= CHASSIS_WITH_SUPPER_CAP;
    }
#endif
}

#ifndef SENTRY_GIMBAL
static void refresh_ECD(void)  // 更新当前云台yawECD值和弧度制的值，实现云台坐标系控制以及小陀螺
{
    #ifdef SENTRY
    robotTotalSpeedControl.gimbalAngleFromChassis=(fp32)betweenTwoSTMMessage_p->yawAngle*PI/(fp32)INT16_MAX-*yawAngle_p-initYawMid;
    #ifdef SENTRY_CHASSIS
    AGVControl[0].presentAngle=(get_motor_measure_point(CHASSIS_LF_G)->ecd-CHASSIS_LF_G_MID_ECD)* 2 * PI / ECD_FULL_ROUND;
    AGVControl[1].presentAngle=(get_motor_measure_point(CHASSIS_RF_G)->ecd-CHASSIS_RF_G_MID_ECD)* 2 * PI / ECD_FULL_ROUND;
    AGVControl[2].presentAngle=(get_motor_measure_point(CHASSIS_RB_G)->ecd-CHASSIS_RB_G_MID_ECD)* 2 * PI / ECD_FULL_ROUND;
    AGVControl[3].presentAngle=(get_motor_measure_point(CHASSIS_LB_G)->ecd-CHASSIS_LB_G_MID_ECD)* 2 * PI / ECD_FULL_ROUND;
    #endif
    #else
    robotTotalSpeedControl.nowYawECD = get_motor_measure_point(YAW_GM)->ecd;
    robotTotalSpeedControl.gimbalAngleFromChassis = (robotTotalSpeedControl.nowYawECD - YAW_MID_ECD) * 2 * PI / ECD_FULL_ROUND;
    #endif
}

static void calc_wheel_rpm(void)  // 计算轮子转动速度(rpm)
{
    fp32 sin_yaw, cos_yaw;
    fp32 vx, vy, w;
    sin_yaw = arm_sin_f32(robotTotalSpeedControl.gimbalAngleFromChassis);
    cos_yaw = arm_cos_f32(robotTotalSpeedControl.gimbalAngleFromChassis);
    // 将云台坐标系速度转化为底盘坐标系速度。坐标系变换
    vx = cos_yaw * robotTotalSpeedControl.vx - sin_yaw * robotTotalSpeedControl.vy;
    vy = sin_yaw * robotTotalSpeedControl.vx + cos_yaw * robotTotalSpeedControl.vy;
    w = robotTotalSpeedControl.w;
#ifdef OMNIDIRECTIONAL_WHEELS
    fp32 vw = CHASSIS_R * w;
    driveMotor[0].wantedMotorSpeed = ((vx - vw) * CHASSIS_THETA - (vy + vw) * CHASSIS_THETA) / WHEEL_PERIMETER;
    driveMotor[1].wantedMotorSpeed = -((vx + vw) * CHASSIS_THETA + (vy + vw) * CHASSIS_THETA) / WHEEL_PERIMETER;
    driveMotor[2].wantedMotorSpeed = -((vx + vw) * CHASSIS_THETA - (vy - vw) * CHASSIS_THETA) / WHEEL_PERIMETER;
    driveMotor[3].wantedMotorSpeed = ((vx - vw) * CHASSIS_THETA + (vy - vw) * CHASSIS_THETA) / WHEEL_PERIMETER;
#endif
#ifdef MECANUM_WHEELS
    driveMotor[0].wantedMotorSpeed = (vx - vy - w * CHASSIS_RX_ADD_RY) / WHEEL_PERIMETER;
    driveMotor[1].wantedMotorSpeed = (-vx - vy - w * CHASSIS_RX_ADD_RY) / WHEEL_PERIMETER;
    driveMotor[2].wantedMotorSpeed = (-vx + vy - w * CHASSIS_RX_ADD_RY) / WHEEL_PERIMETER;
    driveMotor[3].wantedMotorSpeed = (vx + vy - w * CHASSIS_RX_ADD_RY) / WHEEL_PERIMETER;
#endif
#ifdef STEERING_WHEELS
    fp32 vw = CHASSIS_R * CHASSIS_THETA * w;
    AGVControl[0].vx = vx - vw;
    AGVControl[0].vy = vy + vw;
    AGVControl[1].vx = vx + vw;
    AGVControl[1].vy = vy + vw;
    AGVControl[2].vx = vx + vw;
    AGVControl[2].vy = vy - vw;
    AGVControl[3].vx = vx - vw;
    AGVControl[3].vy = vy - vw;
    for (int i = 0; i < 4; i++) {
        if (AGVControl[i].vx > 0) {
            AGVControl[i].directionFlag = 1;
        } else if (AGVControl[i].vx == 0) {
            AGVControl[i].directionFlag = 0;
        } else {
            AGVControl[i].directionFlag = -1;
            AGVControl[i].vx *= -1;
            AGVControl[i].vy *= -1;
        }
    }
    for (int i = 0; i < 4; i++) {
        if (i == 1 || i == 2) {
            AGVControl[i].wantedMotorSpeed = -sqrt(AGVControl[i].vx * AGVControl[i].vx + AGVControl[i].vy * AGVControl[i].vy) / WHEEL_PERIMETER;
        }
        if (i == 0 || i == 3) {
            AGVControl[i].wantedMotorSpeed = sqrt(AGVControl[i].vx * AGVControl[i].vx + AGVControl[i].vy * AGVControl[i].vy) / WHEEL_PERIMETER;
        }
        if (AGVControl[i].vx == 0 && AGVControl[i].vy == 0) {
            AGVControl[i].wantedAngle = 0;
        } else {
            AGVControl[i].wantedAngle = atan2f(AGVControl[i].vy, AGVControl[i].vx);
        }
    }
#endif
}

#ifdef STEERING_WHEELS
fp32 radFormat(fp32 rawAngle, int8_t* flag)  // 将角度化为(-PI/2,PI/2)范围内
{
    while (rawAngle > PI)
        rawAngle -= (2 * PI);
    while (rawAngle < (-PI))
        rawAngle += (2 * PI);
    if (rawAngle > PI / 2) {
        rawAngle -= PI;
        *flag = *flag * (-1);
    } else if (rawAngle < (-PI / 2)) {
        rawAngle += PI;
        *flag = *flag * (-1);
    }

    return rawAngle;
}

void GM6020_PID_calc(pid_type_def* pid, fp32 ref, fp32 set, int8_t* flag) {
    if (pid == NULL) {
        pid->out = 0;
    }

    pid->error[2] = pid->error[1];
    pid->error[1] = pid->error[0];
    pid->set = set;
    pid->fdb = ref;
    pid->error[0] = radFormat(set - ref, flag);
    if (pid->mode == PID_POSITION) {
        pid->Pout = pid->Kp * pid->error[0];
        pid->Iout += pid->Ki * pid->error[0];
        pid->Dbuf[2] = pid->Dbuf[1];
        pid->Dbuf[1] = pid->Dbuf[0];
        pid->Dbuf[0] = (pid->error[0] - pid->error[1]);
        pid->Dout = pid->Kd * pid->Dbuf[0];
        range_limit_inside(pid->Iout, pid->max_iout);
        pid->out = pid->Pout + pid->Iout + pid->Dout;
        range_limit_inside(pid->out, pid->max_out);
    } else if (pid->mode == PID_DELTA) {
        pid->Pout = pid->Kp * (pid->error[0] - pid->error[1]);
        pid->Iout = pid->Ki * pid->error[0];
        pid->Dbuf[2] = pid->Dbuf[1];
        pid->Dbuf[1] = pid->Dbuf[0];
        pid->Dbuf[0] = (pid->error[0] - 2.0f * pid->error[1] + pid->error[2]);
        pid->Dout = pid->Kd * pid->Dbuf[0];
        pid->out += pid->Pout + pid->Iout + pid->Dout;
        range_limit_inside(pid->out, pid->max_out);
    }
}
uint16_t M1,M2,M3,M4;

void calc_give_current(void)  // pid 计算电流
{
    uint8_t i;
#ifdef STEERING_WHEELS
    for (i = 0; i < 4; i++) {
        AGVControl[i].presentMotorSpeed_GM = get_motor_measure_point(i + CHASSIS_LF_G)->speed_rpm;
        GM6020_PID_calc(&(driveMotor_GM[i].vpidOfRpm), -AGVControl[i].presentAngle, AGVControl[i].wantedAngle, &(AGVControl[i].directionFlag));
        PID_calc(&(driveMotor_GM[i].vpidOfVolage), -AGVControl[i].presentMotorSpeed_GM, driveMotor_GM[i].vpidOfRpm.out);
        driveMotor_GM[i].giveCurrent = -driveMotor_GM[i].vpidOfVolage.out;
    }
    for (i = 0; i < 4; i++) {
        AGVControl[i].presentMotorSpeed = ((fp32)(get_motor_measure_point(i+CHASSIS_LF_M)->speed_rpm)) / M3508_K;
        first_order_filter_cali(&(AGVControl[i].M3508SpeedFilter),AGVControl[i].presentMotorSpeed);
        AGVControl[i].presentMotorSpeed=AGVControl[i].M3508SpeedFilter.out;
        PID_calc(&(driveMotor_M[i].vpid), AGVControl[i].presentMotorSpeed, AGVControl[i].wantedMotorSpeed * AGVControl[i].directionFlag);  // pid计算
        driveMotor_M[i].giveCurrent = ((driveMotor_M[i].vpid.out>500||driveMotor_M[i].vpid.out<-500)?driveMotor_M[i].vpid.out:0);   
    }
#endif
}

#else

static void calc_give_current(void)  // pid 计算电流
{
    uint8_t i;
    for (i = 0; i < 4; i++) {
        driveMotor[i].presentMotorSpeed = get_motor_measure_point(i+CHASSIS_LF_M)->speed_rpm / M3508_K;         // 获取电机转速
        PID_calc(&(driveMotor[i].vpid), driveMotor[i].presentMotorSpeed, driveMotor[i].wantedMotorSpeed);  // pid计算
        driveMotor[i].giveCurrent = driveMotor[i].vpid.out;                                                // pid的输出就是驱动电流
    }
}
#endif

#ifdef STM_STUDIO_POWER_CONTROL
fp32 scale;
uint16_t powerLimit;
#endif

#ifdef WITH_REFEREE

static void power_control(void)  // 2.3，对计算得到的发送电流进行进一步的功率控制
{
    get_chassis_power_and_buffer(&powerNow, &bufferNow);  // 从裁判系统获取当前功率和缓冲能量

    #ifndef STM_STUDIO_POWER_CONTROL
    uint16_t powerLimit;
    #endif
    get_power_limit(&powerLimit);  // 从裁判系统读取当前功率上限
    powerLimit -= 4;
    fp32 halfPowerLimit;  //功率转折点，
    halfPowerLimit=(fp32)powerLimit/3;
    #ifndef STEERING_WHEELS
    fp32 totalCurrentAfterPID = 0.0f;  // 经过上一步PID计算之后得到的想要发送的底盘总电流
    #else
    fp32 totalCurrentAfterPID_M = 0.0f;  // 经过上一步PID计算之后得到的想要发送M3508总电流
    fp32 totalCurrentAfterPID_GM = 0.0f;
    #endif
    uint8_t i;
    #ifndef STEERING_WHEELS
    for (i = 0; i < 4; i++) {
        int16_t absCurrent;
        abss(driveMotor[i].giveCurrent, absCurrent);
        totalCurrentAfterPID += absCurrent;
    }
    #else
    for (i = 0; i < 4; i++) {
        int16_t absCurrent;
        abss(driveMotor_M[i].giveCurrent, absCurrent);
        totalCurrentAfterPID_M += absCurrent;
    }
    for (i = 0; i < 4; i++) {
        int16_t absCurrent;
        abss(driveMotor_GM[i].giveCurrent, absCurrent);
        totalCurrentAfterPID_GM += absCurrent;
    }
    #endif
    if (bufferNow > DANGEROUS_BUFFER)  // 如果还没有碰到危险线
        totalCurrentLimit = ((fp32)powerLimit + (bufferNow - DANGEROUS_BUFFER) / COMMUNICATING_PERIOD_S) / VOLTAGE * CURRENT_TO_DATA;
    else  // 如果已经碰到危险线，那就老老实实用最大功率
        totalCurrentLimit = (fp32)powerLimit / VOLTAGE * CURRENT_TO_DATA;
    // 下面根据上限提出的要求按需缩放发送电流
    #ifndef STEERING_WHEELS
    if (totalCurrentAfterPID > totalCurrentLimit) {
        #ifndef STM_STUDIO_POWER_CONTROL
        fp32 scale = totalCurrentLimit / totalCurrentAfterPID;
        #else
        scale = totalCurrentLimit / totalCurrentAfterPID;
        #endif
        uint8_t i;
        for (i = 0; i < 4; i++)
            driveMotor[i].giveCurrent *= scale;
    }
    #else
    if (totalCurrentAfterPID_GM >= totalCurrentLimit) { //优先保留转向功率
        #ifndef STM_STUDIO_POWER_CONTROL
        fp32 scale = totalCurrentLimit / (totalCurrentAfterPID_GM+totalCurrentAfterPID_M);
        #else
        scale = totalCurrentLimit / (totalCurrentAfterPID_GM+totalCurrentAfterPID_M);
        #endif
        uint8_t i;
        for (i = 0; i < 4; i++){
            driveMotor_M[i].giveCurrent *= scale;
        }
        for (i = 0; i < 4; i++){
            driveMotor_GM[i].giveCurrent *= scale;
        }
    }else{
        #ifndef STM_STUDIO_POWER_CONTROL
        fp32 scale = (totalCurrentLimit-totalCurrentAfterPID_GM) / totalCurrentAfterPID_M;
        #else
        scale = totalCurrentLimit / (totalCurrentAfterPID_GM+totalCurrentAfterPID_M);
        #endif
        uint8_t i;
        for (i = 0; i < 4; i++){
            driveMotor_M[i].giveCurrent *= scale;
        }
    }
    #endif
}
#endif
#endif

void chassis_task(void const* pvParameters) {
    osDelay(CHASSIS_TASK_INIT_TIME);  // 等待电机初始化
    PIDs_init();                      // 初始化PID
    Filters_init();
    rc_p = get_remote_control_point();  // 获取遥控器数据和NUC数据指针
    nuc_p = get_nuc_control_point();
    robotMode = get_robot_present_mode();
    remoteControl_p=get_usart_remote_control_p();

	#ifdef SENTRY
    betweenTwoSTMMessage_p = get_between_two_STM_message_point();
    yawAngle_p=get_INS_angle_point();
    initYawMid=(fp32)betweenTwoSTMMessage_p->yawAngle*PI/(fp32)INT16_MAX-*yawAngle_p;
#endif
    
    while (1) {
        analyse_total_control();
#ifndef SENTRY_GIMBAL
        refresh_ECD(); 
        first_order_filt();
        calc_wheel_rpm();
        calc_give_current();
#ifdef WITH_REFEREE
        power_control();  // 2.3，功率控制
#endif
#ifdef ZERO_CURRENT_SAFE_CHASSIS
        zeroCurrentMark=1;
#endif
#ifdef STEERING_WHEELS
        if (zeroCurrentMark) {
            CAN_cmd_chassis_M3508(0, 0, 0, 0);  // 发送零电流
            osDelay(CHASSIS_CONTROL_TIME_MS);
            CAN_cmd_chassis_GM6020  (0, 0, 0, 0);
        } else { 
            CAN_cmd_chassis_GM6020(driveMotor_GM[0].giveCurrent, driveMotor_GM[1].giveCurrent, driveMotor_GM[2].giveCurrent, driveMotor_GM[3].giveCurrent);
            CAN_cmd_chassis_M3508(driveMotor_M[0].giveCurrent, driveMotor_M[1].giveCurrent, driveMotor_M[2].giveCurrent, driveMotor_M[3].giveCurrent);  // 正常控制电机
            osDelay(CHASSIS_CONTROL_TIME_MS);
        }
#else
        if (zeroCurrentMark)
            CAN_cmd_chassis(0, 0, 0, 0);  // 发送零电流
        else
            CAN_cmd_chassis(driveMotor[0].giveCurrent, driveMotor[1].giveCurrent, driveMotor[2].giveCurrent, driveMotor[3].giveCurrent);  // 正常控制电机

#endif
#else
        betweenTwoSTMMessage_p->x = (int8_t)(robotTotalSpeedControl.vx / CHASSIS_MAX_SPEED * (fp32)INT8_MAX);
        betweenTwoSTMMessage_p->y = (int8_t)(robotTotalSpeedControl.vy / CHASSIS_MAX_SPEED * (fp32)INT8_MAX);
        betweenTwoSTMMessage_p->w = (int8_t)(robotTotalSpeedControl.w / CHASSIS_MAX_OMEGA * (fp32)INT8_MAX);
        betweenTwoSTMMessage_p->nextmode = *robotMode;
        betweenTwoSTMMessage_p->yawAngle =(int16_t) (*yawAngle_p/PI*(fp32)INT16_MAX);
#endif
        osDelay(CHASSIS_CONTROL_TIME_MS);
    }
}
